/* system */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <algorithm>

/* OpenCV */
#include <opencv2/opencv.hpp>

#include "epipolar.h"
#include "sba.h"

template<typename T> int CompressPoints( T* ptr, const uchar* mask, int mstep, int count )
{
    int i, j;
    for( i = j = 0; i < count; i++ )
        if( mask[i*mstep] )
        {
            if( i > j )
                ptr[j] = ptr[i];
            j++;
        }
    return j;
}
// returns squared distance between two 2D points with floating-point coordinates.
double SqDist2D32f( CvPoint2D32f pt1, CvPoint2D32f pt2 )
{
    double dx = pt1.x - pt2.x;
    double dy = pt1.y - pt2.y;

    return dx*dx + dy*dy;
}
/* Print out the content of the matrix */
void PrintMatrix(CvMat *mat)
{
	int channel = mat->step / (mat->cols * sizeof(double));

	for (int i = 0; i < mat->rows; i++)
	{
		for (int j = 0; j < mat->cols; j++)
		{
			for (int k = 0; k < channel; k++)
			{
				printf("%f%s", mat->data.db[i * mat->cols * channel + j * channel + k], j == mat->cols - 1 ? "\n" : " ");
			}
		}
	}
}
/***************************** Projective Reconstruction ****************************/
/*
	Two view reconstruction
*/

// projective function and its jacobian
void proj_func(double *p, struct sba_crsm *idxij, int *wk1, int *wk2, double *hx, void *adata)
{
	int m = *(int *)adata;
	int n = *((int *)adata + 1);

	CvMat* P = cvCreateMat(3, 4, CV_64FC1);
	CvMat* M = cvCreateMat(4, 1, CV_64FC1);

	for (int j = 0; j < m; j++)
	{
		for (int i = 0; i < n; i++)
		{
			double xij[3];
			//project3dto2d(p + j * 12, p + m * 12 + i * 4, 1, xij);
			CvMat X = cvMat(3, 1, CV_64F, xij);
			
			for (int k = 0; k < 11; k++)
			{
				P->data.db[k] = p[j*11 + k];
			}
			P->data.db[11] = 1.;
			
			for (int k = 0; k < 3; k++)
			{
				M->data.db[k] = p[m*11 + i*3 + k];
			}
			M->data.db[3] = 1.;

			cvMatMul(P, M, &X);

			hx[i * m * 2 + j * 2] = xij[0] / xij[2];
			hx[i * m * 2 + j * 2 + 1] = xij[1] / xij[2];
		}
	}

	cvReleaseMat(&P);
	cvReleaseMat(&M);
}
void proj_fjac(double *p, struct sba_crsm *idxij, int *wk1, int *wk2, double *jac, void *adata)
{
	int m = *(int *)adata;
	int n = *((int *)adata + 1);
	int cnp = 11;
	int pnp = 3;
	int mnp = 2;

	for (int j = 0; j < m; j++)
	{
		for (int i = 0; i < n; i++)
		{
			CvMat *aj = cvCreateMat(3, 4, CV_64FC1);
			for (int k = 0; k < 3; k++)
			{
				for (int l = 0; l < 4; l++)
				{
					aj->data.db[k * 4 + l] = p[j * cnp + k * pnp + l];
				}
			}
			CvMat *bi = cvCreateMat(4, 1, CV_64FC1);
			for (int k = 0; k < 4; k++)
			{
				bi->data.db[k] = p[m * cnp + i * pnp + k];
			}
			CvMat *xij = cvCreateMat(3, 1, CV_64FC1);
			cvMatMul(aj, bi, xij);
			
			if (xij->data.db[2] > DBL_EPSILON)
			{
				double *ptr = jac + i * (mnp * cnp + mnp * pnp) * m + j * (mnp * cnp + mnp * pnp);
				
				for (int k = 0; k < 4; k++)
				{
					ptr[k] = bi->data.db[k] / xij->data.db[2];
				}
				for (int k = 4; k < 8; k++)
				{
					ptr[k] = 0.0;
				}
				for (int k = 8; k < 12; k++)
				{
					ptr[k] = -xij->data.db[0] / (xij->data.db[2] * xij->data.db[2]) * bi->data.db[k % 4];
				}
				
				ptr += cnp;
				
				for (int k = 0; k < 4; k++)
				{
					ptr[k] = 0.0;
				}
				for (int k = 4; k < 8; k++)
				{
					ptr[k] = bi->data.db[k % 4] / xij->data.db[2];
				}
				for (int k = 8; k < 12; k++)
				{
					ptr[k] = -xij->data.db[1] / (xij->data.db[2] * xij->data.db[2]) * bi->data.db[k % 4];
				}

				ptr += cnp;

				for (int k = 0; k < 4; k++)
				{
					ptr[k] = aj->data.db[k] / xij->data.db[2] - xij->data.db[0] / (xij->data.db[2] * xij->data.db[2]) * aj->data.db[k + 8];
				}
				
				ptr += pnp;

				for (int k = 0; k < 4; k++)
				{
					ptr[k] = aj->data.db[k + 4] / xij->data.db[2] - xij->data.db[0] / (xij->data.db[2] * xij->data.db[2]) * aj->data.db[k + 8];
				}
			}
			else
			{
				printf("proj_fjac: xij is at infinity!\n");
				exit(-1);
			}
			cvReleaseMat(&aj);
			cvReleaseMat(&bi);
			cvReleaseMat(&xij);
		}
	}
}
int HartleyProjectReconstrFrom2View(const CvMat* points1, const CvMat* points2, 
									 CvMat* projMatr1, CvMat* projMatr2, CvMat* points4D)
{	
	/* points1, points2 must be 3xN, points4D must be 4xN */
	int count;
	count = points1->cols;
	if (count < 8)
		return 0;

	/* Compute fundamental matrix F */
	double f[9];
	CvMat F = cvMat(3, 3, CV_64F, f);
	// assume points1 and points2 have already been verified.
	int f_count = cvFindFundamentalMat(points1, points2, &F, CV_FM_RANSAC, 1.0, 0.99, 0);
	if (f_count == 0)
		return 0;

	/* compute right epipole from F' * e2 = 0 */
	double ft[9];
	CvMat Ft = cvMat(3, 3, CV_64F, ft);
	double e2_dat[3];
	CvMat e2 = cvMat(3, 1, CV_64F, e2_dat);
	CvMat *S, *U, *V;
	S = cvCreateMat(3, 3, CV_64FC1);
	U = cvCreateMat(3, 3, CV_64FC1);
	V = cvCreateMat(3, 3, CV_64FC1);
	double scale, *v = V->data.db;

	cvTranspose(&F, &Ft);
	cvSVD(&Ft, S, U, V, CV_SVD_MODIFY_A + CV_SVD_V_T);
	scale = sqrt(v[2*3 + 0]*v[2*3 + 0] + v[2*3 + 1]*v[2*3 + 1]);
	e2_dat[0] = v[2*3 + 0] / scale;
	e2_dat[1] = v[2*3 + 1] / scale;
	e2_dat[2] = v[2*3 + 2] / scale;
	if (e2_dat[2] < 0)
	{
		e2_dat[0] = -e2_dat[0];
		e2_dat[1] = -e2_dat[1];
		e2_dat[2] = -e2_dat[2];
	}
	cvReleaseMat(&S); cvReleaseMat(&U); cvReleaseMat(&V);

	/* Factorize F as F = [e2]xM */
	double e2x[] = {
	0., -e2_dat[2], e2_dat[1],
	e2_dat[2], 0., -e2_dat[0],
	-e2_dat[1], e2_dat[0], 0.
	};
	CvMat e2x_ = cvMat(3, 3, CV_64F, e2x);
	CvMat* e2x_i = cvCreateMat(3, 3, CV_64FC1);
	double m[3*3];
	CvMat M = cvMat(3, 3, CV_64F, m);

	// invert [e2]x
	if (cvInvert(&e2x_, e2x_i, CV_LU) == 0)
	{
		cvReleaseMat(&e2x_i);
		return 0;
	}
	// left multiply F by e2x_i
	cvMatMul(e2x_i, &F, &M);

	cvReleaseMat(&e2x_i);

	/* Mu = u' - e2, x = [u 1]', P1 = [I 0], P2 = [M e2] */
	double *P1 = projMatr1->data.db, *P2 = projMatr2->data.db;
	double *x = points4D->data.db, *u = points1->data.db;
	int i, j;
	
	// fill in projection matrices
	cvSetZero(projMatr1);
	P1[0 * 4 + 0] = P1[1 * 4 + 1] = P1[2 * 4 + 2] = 1.;
	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			P2[i * 4 + j] = m[i * 3 + j];
		}
		P2[i * 4 + 3] = e2_dat[i];
	}
	// fill in reconstructed 4D points
	for (i = 0; i < count; i++)
	{
		x[0 * count + i] = u[0 * count + i];
		x[1 * count + i] = u[1 * count + i];
		x[2 * count + i] = u[2 * count + i];
		x[3 * count + i] = 1.;
	}

	return 1;
}
void Make3DPoints(const CvMat* points2D, CvMat* points3D)
{
	int numPoints, i;
	double *p2, *p3;

	numPoints = points2D->cols;
	p2 = points2D->data.db;
	p3 = points3D->data.db;
	for (i = 0; i < numPoints; i++)
	{
		p3[0 * numPoints + i] = p2[0 * numPoints + i];
		p3[1 * numPoints + i] = p2[1 * numPoints + i];
		p3[2 * numPoints + i] = 1.;
	}
}
void ComputeProjectMatrix(const CvMat* objPoints, CvMat* projPoints, CvMat* projMatr)
{/* Using SVD method */

    /* Reconstruct points using object points and projected points */
    /* Number of points must be >=6 */

    CvMat matrV;
    CvMat* matrA = 0;
    CvMat* matrW = 0;
    CvMat* workProjPoints = 0;
    CvMat* tmpProjPoints = 0;

    int numPoints;
    numPoints = projPoints->cols;

    /* Create and fill matrix A */
    matrA = cvCreateMat(numPoints*3, 12, CV_64F);
    matrW = cvCreateMat(numPoints*3, 12, CV_64F);

    if( projPoints->rows == 2 )
    {
        tmpProjPoints = cvCreateMat(3,numPoints,CV_64F);
        Make3DPoints(projPoints,tmpProjPoints);
        workProjPoints = tmpProjPoints;
    }
    else
    {
        workProjPoints = projPoints;
    }

    double matrV_dat[144];
    matrV = cvMat(12,12,CV_64F,matrV_dat);
    int i;

    char* dat;
    dat = (char*)(matrA->data.db);

#if 1
    FILE *file;
    file = fopen("d:\\test\\recProjMatr.txt","w");

#endif
    for( i = 0;i < numPoints; i++ )
    {
        double x,y,w;
        double X,Y,Z,W;
        double*  matrDat = (double*)dat;

        x = cvmGet(workProjPoints,0,i);
        y = cvmGet(workProjPoints,1,i);
        w = cvmGet(workProjPoints,2,i);


        X = cvmGet(objPoints,0,i);
        Y = cvmGet(objPoints,1,i);
        Z = cvmGet(objPoints,2,i);
        W = cvmGet(objPoints,3,i);

#if 1
        fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf %lf)\n",i,X,Y,Z,W,x,y,w );
#endif

/*---*/
        matrDat[ 0] = 0;
        matrDat[ 1] = 0;
        matrDat[ 2] = 0;
        matrDat[ 3] = 0;

        matrDat[ 4] = -w*X;
        matrDat[ 5] = -w*Y;
        matrDat[ 6] = -w*Z;
        matrDat[ 7] = -w*W;

        matrDat[ 8] = y*X;
        matrDat[ 9] = y*Y;
        matrDat[10] = y*Z;
        matrDat[11] = y*W;
/*---*/
        matrDat[12] = w*X;
        matrDat[13] = w*Y;
        matrDat[14] = w*Z;
        matrDat[15] = w*W;

        matrDat[16] = 0;
        matrDat[17] = 0;
        matrDat[18] = 0;
        matrDat[19] = 0;

        matrDat[20] = -x*X;
        matrDat[21] = -x*Y;
        matrDat[22] = -x*Z;
        matrDat[23] = -x*W;
/*---*/
        matrDat[24] = -y*X;
        matrDat[25] = -y*Y;
        matrDat[26] = -y*Z;
        matrDat[27] = -y*W;

        matrDat[28] = x*X;
        matrDat[29] = x*Y;
        matrDat[30] = x*Z;
        matrDat[31] = x*W;

        matrDat[32] = 0;
        matrDat[33] = 0;
        matrDat[34] = 0;
        matrDat[35] = 0;
/*---*/
        dat += (matrA->step)*3;
    }
#if 1
    fclose(file);

#endif

    /* Solve this system */

    /* From svd we need just last vector of V or last row V' */
    /* We get transposed matrix V */

    cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T);

    /* projected matrix was computed */
    for( i = 0; i < 12; i++ )
    {
        cvmSet(projMatr,i/4,i%4,cvmGet(&matrV,11,i));
    }

    cvReleaseMat(&matrA);
    cvReleaseMat(&matrW);
    cvReleaseMat(&tmpProjPoints);
}
void ComputeProjectMatrixStatus(const CvMat *objPoints4D, const CvMat *points2, const CvMat *status, CvMat *projMatr)
{
    /* Compute number of good points */
    int num = cvCountNonZero(status);
    
    /* Create arrays */
    CvMat *objPoints = 0;
    objPoints = cvCreateMat(4,num,CV_64F);

    CvMat *points2D = 0;
    points2D = cvCreateMat(2,num,CV_64F);

    int currVis = 0;
    int i;
#if 1
    FILE *file;
    file = fopen("d:\\test\\projStatus.txt","w");
#endif
    int totalNum = objPoints4D->cols;
    for( i = 0; i < totalNum; i++ )
    {
        fprintf(file,"%d (%d) ",i,status->data.ptr[i]);
        if( status->data.ptr[i] )
        {

#if 1
            double X,Y,Z,W;
            double x,y;
            X = cvmGet(objPoints4D,0,i);
            Y = cvmGet(objPoints4D,1,i);
            Z = cvmGet(objPoints4D,2,i);
            W = cvmGet(objPoints4D,3,i);

            x = cvmGet(points2,0,i);
            y = cvmGet(points2,1,i);
            fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf)",i,X,Y,Z,W,x,y );
#endif
            cvmSet(objPoints,0,currVis,cvmGet(objPoints4D,0,i));
            cvmSet(objPoints,1,currVis,cvmGet(objPoints4D,1,i));
            cvmSet(objPoints,2,currVis,cvmGet(objPoints4D,2,i));
            cvmSet(objPoints,3,currVis,cvmGet(objPoints4D,3,i));

            cvmSet(points2D,0,currVis,cvmGet(points2,0,i));
            cvmSet(points2D,1,currVis,cvmGet(points2,1,i));

            currVis++;
        }
        
        fprintf(file,"\n");
    }

#if 1
    fclose(file);
#endif

    ComputeProjectMatrix(objPoints,points2D,projMatr);

    /* Free allocated memory */
    cvReleaseMat(&objPoints);
    cvReleaseMat(&points2D);
}
int CreateFeaturePoints(const IplImage *image, CvMat *points, CvMat *status)
{
    int foundFeaturePoints = 0;
    IplImage *grayImage = 0;
    IplImage *eigImage = 0;
    IplImage *tmpImage = 0;
    CvPoint2D32f *cornerPoints = 0;

    /* Test image size */
    int w,h;
    w = image->width;
    h = image->height;

    int needNumPoints;
    needNumPoints = points->cols;

    /* Create temporary images */
    grayImage = cvCreateImage(cvSize(w,h), 8, 1);
    eigImage = cvCreateImage(cvSize(w,h), 32, 1);
    tmpImage = cvCreateImage(cvSize(w,h), 32, 1);

    /* Create points */
    cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints);

    int foundNum;
    double quality;
    double minDist;

    cvCvtColor(image,grayImage, CV_BGR2GRAY);

    foundNum = needNumPoints;
    quality = 0.01;
    minDist = 5;
    cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist);

    /* Copy found points to result */
    int i;
    for( i = 0; i < foundNum; i++ )
    {
        cvmSet(points,0,i,cornerPoints[i].x);
        cvmSet(points,1,i,cornerPoints[i].y);
    }

    /* Set status if need */
    if( status )
    {
        for( i = 0; i < foundNum; i++ )
        {
            status->data.ptr[i] = 1;
        }

        for( i = foundNum; i < needNumPoints; i++ )
        {
            status->data.ptr[i] = 0;
        }
    }

    foundFeaturePoints = foundNum;

    /* Free allocated memory */
    cvReleaseImage(&grayImage);
    cvReleaseImage(&eigImage);
    cvReleaseImage(&tmpImage);
    cvFree(&cornerPoints);

    return foundFeaturePoints;
}
/* For given points1 (with pntStatus) on frame1 finds corresponding points2 on frame2 and set pntStatus2 for them */
/* Returns number of corresponding points */
int FindCorrForGivenPoints(		CvCapture *video,
								const int frame1,/* Frame 1 */
                                const int frame2,/* Frame 2 */
                                const CvMat *points1, 
                                const CvMat *pntStatus1,
                                CvMat *points2,
                                CvMat *pntStatus2,
                                const int useFilter,/*Use fundamental matrix to filter points */
                                const double threshold)/* Threshold for good points in filter */
{
	int resNumCorrPoints = 0;
    CvPoint2D32f* cornerPoints1 = 0;
    CvPoint2D32f* cornerPoints2 = 0;
    char*  status = 0;
    float* errors = 0;
    CvMat* tmpPoints1 = 0;
    CvMat* tmpPoints2 = 0;
    CvMat* pStatus = 0;
    IplImage *grayImage1 = 0;
    IplImage *grayImage2 = 0;
    IplImage *pyrImage1 = 0;
    IplImage *pyrImage2 = 0;

 
    /* Test image size */
    int w,h;
	IplImage* image1;
	cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, frame1);
	image1 = cvQueryFrame(video);
    w = image1->width;
    h = image1->height;

 
    /* Test number of points */
    int numPoints;
    numPoints = points1->cols;

    /* Compute number of visible points on image1 */
    int numVisPoints;
    numVisPoints = cvCountNonZero(pntStatus1);

	int i;

    if( numVisPoints > 0 )
    {
        /* Create Ipl images */
        grayImage1 = cvCreateImage(cvSize(w,h),8,1);
        grayImage2 = cvCreateImage(cvSize(w,h),8,1);
        pyrImage1  = cvCreateImage(cvSize(w+3,h),8,1);
        pyrImage2  = cvCreateImage(cvSize(w+3,h),8,1);

		/* variables for the loop */
		int step, currFrame;
		step = frame1 < frame2 ? 1 : -1;
		currFrame = frame1;

		CvMat *currPoints1, *currPntStatus1;
		currPoints1 = cvCloneMat(points1);
		currPntStatus1 = cvCloneMat(pntStatus1);
		CvMat *currPoints2, *currPntStatus2;
		currPoints2 = points2;
		currPntStatus2 = pntStatus2;

		int currNumVisPoints;
		currNumVisPoints = numVisPoints;

		/* grab current frame before doing the loop */
		cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, currFrame);
		cvCvtColor(cvQueryFrame(video),grayImage1,CV_BGR2GRAY);

		while (currFrame != frame2)
		{
			/* grab the next frame */
			cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, currFrame + step);
			cvCvtColor(cvQueryFrame(video),grayImage2,CV_BGR2GRAY);

			/* allocate memory for LK optical flow */
			cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*currNumVisPoints);
			cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*currNumVisPoints);
			status = (char*)cvAlloc( sizeof(char)*currNumVisPoints);
			errors = (float*)cvAlloc( 2 * sizeof(float)*currNumVisPoints);

			for( i = 0; i < currNumVisPoints; i++ )
			{
				status[i] = 1;
			}

			/* Copy points good points from input data */
			uchar *stat1 = currPntStatus1->data.ptr;
			uchar *stat2 = currPntStatus2->data.ptr;
			int curr = 0;
			for( i = 0; i < numPoints; i++ )
			{
				if( stat1[i] )
				{
					cornerPoints1[curr].x = (float)cvmGet(currPoints1,0,i);
					cornerPoints1[curr].y = (float)cvmGet(currPoints1,1,i);
					curr++;
				}
			}

			/* Define number of levels of pyramid */
			if (currFrame == frame1)
				cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
										pyrImage1, pyrImage2,
										cornerPoints1, cornerPoints2,
										currNumVisPoints, cvSize(10,10), 3,
										status, errors, 
										cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
										0/*CV_LKFLOW_PYR_A_READY*/ );
			else
				cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
										pyrImage1, pyrImage2,
										cornerPoints1, cornerPoints2,
										currNumVisPoints, cvSize(10,10), 3,
										status, errors, 
										cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
										CV_LKFLOW_PYR_A_READY );

			memset(stat2,0,sizeof(uchar)*numPoints);

	        int currVis = 0;
		    int totalCorns = 0;

			/* Copy new points and set status */
			/* stat1 may not be the same as stat2 */
			for( i = 0; i < numPoints; i++ )
			{
				if( stat1[i] )
				{
					if( status[currVis] && errors[currVis] < 1000 )
					{
						stat2[i] = 1;
						cvmSet(currPoints2,0,i,cornerPoints2[currVis].x);
						cvmSet(currPoints2,1,i,cornerPoints2[currVis].y);
						totalCorns++;
					}
					currVis++;
				}
			}

			/* prepare for the next iteration */
			currFrame += step;
			swap(grayImage1, grayImage2);
			swap(pyrImage1, pyrImage2);
			swap(currPoints1, currPoints2);
			swap(currPntStatus1, currPntStatus2);
			currNumVisPoints = cvCountNonZero(currPntStatus1);

			/* free allocated memory */
			cvFree(&cornerPoints1);
			cvFree(&cornerPoints2);
			cvFree(&status);
			cvFree(&errors);
		}
        
		/* copy the results to points2 and pntStatus2 */
		cvCopy(currPoints1, points2);
		cvCopy(currPntStatus1, pntStatus2);

        resNumCorrPoints = currNumVisPoints;

        /* Filter points using RANSAC */
        if( useFilter )
        {
            resNumCorrPoints = 0;
			
			uchar* stat2 = pntStatus2->data.ptr;

            /* Use RANSAC filter for found points */
            if( currNumVisPoints > 7 )
            {
                /* Create array with good points only */
                tmpPoints1 = cvCreateMat(2,currNumVisPoints,CV_64F);
                tmpPoints2 = cvCreateMat(2,currNumVisPoints,CV_64F);

                /* Copy just good points */
                int currPoint = 0;
                for( i = 0; i < numPoints; i++ )
                {
                    if( stat2[i] )
                    {
                        cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i));
                        cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i));

                        cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i));
                        cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i));

                        currPoint++;
                    }
                }

                /* Compute fundamental matrix */
                CvMat fundMatr;
                double fundMatr_dat[9];
                fundMatr = cvMat(3,3,CV_64F,fundMatr_dat);
        
                pStatus = cvCreateMat(1,currNumVisPoints,CV_32F);

                int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus);
                if( num > 0 )
                {
                    int curr = 0;
                    /* Set final status for points2 */
                    for( i = 0; i < numPoints; i++ )
                    {
                        if( stat2[i] )
                        {
                            if( cvmGet(pStatus,0,curr) == 0 )
                            {
                                stat2[i] = 0;
                            }
                            curr++;
                        }
                    }
                    resNumCorrPoints = curr;
                }
            }
        }
    }

    /* Free allocated memory */
    cvFree(&cornerPoints1);
    cvFree(&cornerPoints2);
    cvFree(&status);
    cvFree(&errors);
    cvFree(&tmpPoints1);
    cvFree(&tmpPoints2);
    cvReleaseMat( &pStatus );
    cvReleaseImage( &grayImage1 );
    cvReleaseImage( &grayImage2 );
    cvReleaseImage( &pyrImage1 );
    cvReleaseImage( &pyrImage2 );

    return resNumCorrPoints;
}
/* Remove double points */
int RemoveDoublePoints(		const CvMat *oldPoints,/* Points on prev image */
                            const CvMat *newPoints,/* New points */
                            const CvMat *oldStatus,/* Status for old points */
                            const CvMat *newStatus,
                            CvMat *origStatus,
                            const float threshold)/* Status for new points */
{
    CvMemStorage* storage = 0;
    CvSubdiv2D* subdiv = 0;
    CvSeq* seq = 0;

    int originalPoints = 0;
    
    int oldNumPoints;
    oldNumPoints = oldPoints->cols;

    int newNumPoints;
    newNumPoints = newPoints->cols;
    
    /* we have points on image and wants add new points */
    /* use subdivision for find nearest points */

    /* Define maximum and minimum X and Y */
    float minX,minY;
    float maxX,maxY;

    minX = minY = FLT_MAX;
    maxX = maxY = FLT_MIN;

    int i;

    for( i = 0; i < oldNumPoints; i++ )
    {
        if( oldStatus->data.ptr[i] )
        {
            float x = (float)cvmGet(oldPoints,0,i);
            float y = (float)cvmGet(oldPoints,1,i);

            if( x < minX )
                minX = x;

            if( x > maxX )
                maxX = x;

            if( y < minY )
                minY = y;

            if( y > maxY )
                maxY = y;
        }
    }

    for( i = 0; i < newNumPoints; i++ )
    {
        if( newStatus->data.ptr[i] )
        {
            float x = (float)cvmGet(newPoints,0,i);
            float y = (float)cvmGet(newPoints,1,i);

            if( x < minX )
                minX = x;

            if( x > maxX )
                maxX = x;

            if( y < minY )
                minY = y;

            if( y > maxY )
                maxY = y;
        }
    }


    /* Creare subdivision for old image */
    storage = cvCreateMemStorage(0);
//    subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage );
    subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage );
    seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage );

    /* Insert each point from first image */
    for( i = 0; i < oldNumPoints; i++ )
    {
        /* Add just exist points */
        if( oldStatus->data.ptr[i] )
        {
            CvPoint2D32f pt;
            pt.x = (float)cvmGet(oldPoints,0,i);
            pt.y = (float)cvmGet(oldPoints,1,i);

            CvSubdiv2DPoint* point;
            point = cvSubdivDelaunay2DInsert( subdiv, pt );
        }
    }


    /* Find nearest points */
    /* for each new point */
    int flag;
    for( i = 0; i < newNumPoints; i++ )
    {
        flag = 0;
        /* Test just exist points */
        if( newStatus->data.ptr[i] )
        {
            flag = 1;
            /* Let this is a good point */
            //originalPoints++;

            CvPoint2D32f pt;

            pt.x = (float)cvmGet(newPoints,0,i);
            pt.y = (float)cvmGet(newPoints,1,i);

            CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt );

            if( point )
            {
                /* Test distance of found nearest point */
                double minDistance = SqDist2D32f( pt, point->pt );

                if( minDistance < threshold*threshold )
                {
                    /* Point is double. Turn it off */
                    /* Set status */
                    //newStatus->data.ptr[i] = 0;
                    
                    /* No this is a double point */
                    //originalPoints--;
                    flag = 0;
                }
            }
        }
        originalPoints += flag;
        origStatus->data .ptr[i] = (uchar)flag;
    }

    cvReleaseMemStorage( &storage );

    return originalPoints;
}
void AddNewFrameToPrevious(
							CvCapture *video,
							const int newFrame,//Frame to add
                            const int oldFrame,//Previous frame
                            const CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible)
                            const CvMat *oldPntStatus,//Status for each point on prev image
                            const CvMat *objPoints4D,//prev 4D points
                            const CvMat *newPoints,  //Points on new image corr for prev
                            const CvMat *newPntStatus,// New point status for new image
                            CvMat *newFPoints2D1,//new feature points on prev image
                            CvMat *newFPoints2D2,//new feature points on new image
                            CvMat *newFPointsStatus,
                            CvMat *newProjMatr,
                            const int useFilter,
                            const double threshold)//New projection matrix)
{
	IplImage *newImage = 0, *oldImage = 0;

	/* query two frames */
	cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, newFrame);
	newImage = cvCloneImage(cvQueryFrame(video));
	cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, oldFrame);
	oldImage = cvCloneImage(cvQueryFrame(video));

	CvMat *points2 = 0;
    CvMat *status = 0;
    CvMat *newFPointsStatusTmp = 0;

    /* First found correspondence points for images */

    /* Test input params */

    int numPoints;
    numPoints = oldPoints->cols;

    /* Allocate memory */

    /*points2 = cvCreateMat(2,numPoints,CV_64F);
    status = cvCreateMat(1,numPoints,CV_8S);*/
    newFPointsStatusTmp = cvCreateMat(1, newFPoints2D1->cols,CV_8S);

    CvMat projMatr;
    double projMatr_dat[12];
    projMatr = cvMat(3,4,CV_64F,projMatr_dat);

	/* We can compute projection matrix */
    ComputeProjectMatrixStatus(objPoints4D,newPoints,newPntStatus,&projMatr);
    cvCopy(&projMatr,newProjMatr);
        
    /* Create new points and find correspondence */
    CreateFeaturePoints(newImage, newFPoints2D2,newFPointsStatus);
        
    /* Good if we test new points before find corr points */

    /* Find correspondence for new found points */
    FindCorrForGivenPoints( video,
								newFrame,/* Frame 1 */
                                oldFrame,/* Frame 2 */
                                newFPoints2D2,
                                newFPointsStatus,//prev status
                                newFPoints2D1,
                                newFPointsStatusTmp,//new status
                                useFilter,/*Use fundamental matrix to filter points */
                                threshold);/* Threshold for good points in filter */

    /* We generated new points on image test for exist points */

    /* Remove all new double points */

    int origNum;
    /* Find point of old image */
    origNum = RemoveDoublePoints(	oldPoints,/* Points on prev image */
                                    newFPoints2D1,/* New points */
                                    oldPntStatus,/* Status for old points */
                                    newFPointsStatusTmp,
                                    newFPointsStatusTmp,//orig status
                                    20);/* Status for new points */

    /* Find double points on new image */
    origNum = RemoveDoublePoints(	newPoints,/* Points on prev image */
                                    newFPoints2D2,/* New points */
                                    newPntStatus,/* Status for old points */
                                    newFPointsStatusTmp,
                                    newFPointsStatusTmp,//orig status
                                    20);/* Status for new points */



    /* Add all new good points to result */


    /* Copy new status to old */
    cvCopy(newFPointsStatusTmp,newFPointsStatus);

    /* Free allocated memory */
	cvReleaseMat(&newFPointsStatusTmp);

    return;
}
/* Image-based distance from Marc Pollefeys */
double PollefeysImageBasedDist(const CvMat *points1, const CvMat *pntStatus1, const CvMat *points2, const CvMat *pntStatus2)
{
	/* All point matrices are 2xN */

	double dist;

	int count, i, N;
	uchar *stat1, *stat2;

	count = 0;
	N = points1->cols;
	stat1 = pntStatus1->data.ptr;
	stat2 = pntStatus2->data.ptr;

	for (i = 0; i < N; i++)
		count += stat1[i] && stat2[i];

	/* allocate memory */
	CvMat *srcPoints = cvCreateMat(2, count, CV_64FC1);
	CvMat *dstPoints = cvCreateMat(2, count, CV_64FC1);
	CvMat *H = cvCreateMat(3, 3, CV_64FC1);

	double *p1, *p2, *sp, *dp;
	p1 = points1->data.db;
	p2 = points2->data.db;
	sp = srcPoints->data.db;
	dp = dstPoints->data.db;

	int curr = 0;
	/* copy points */
	for (i = 0; i < N; i++)
	{
		if (stat1[i] && stat2[i])
		{
			sp[0*count + curr] = p1[0*N + i];
			sp[1*count + curr] = p1[1*N + i];
			dp[0*count + curr] = p2[0*N + i];
			dp[1*count + curr] = p2[1*N + i];
			curr++;
		}
	}

	cvFindHomography(srcPoints, dstPoints, H, 0);

	/* compute distance between every pair of points */
	CvMat *d_ = cvCreateMat(1, count, CV_64FC1);
	double *d = d_->data.db;

	double Hm_data[3];
	CvMat Hm = cvMat(3, 1, CV_64FC1, Hm_data);
	
	for (i = 0; i < count; i++)
	{
		Hm_data[0] = sp[0*count + i];
		Hm_data[1] = sp[1*count + i];
		Hm_data[2] = 1.;

		cvMatMul(H, &Hm, &Hm);

		double x1 = Hm_data[0] / Hm_data[2];
		double y1 = Hm_data[1] / Hm_data[2];
		double x2 = dp[0*count + i];
		double y2 = dp[1*count + i];

		d[i] = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
	}

	/* find out the median of array d */
	sort(d, d + count);
	dist = d[count/2];

	/* free allocated memory */
	cvReleaseMat(&srcPoints);
	cvReleaseMat(&dstPoints);
	cvReleaseMat(&H);
	cvReleaseMat(&d_);

	return dist;
}
void dlt(const double *x, const double *y, const int N, double *a)
{
	/*
		solve the equations x = Ay to a similarity for A
		x: 2x1 vector, y: 3x1 vector, A: 2x3 matrix, N: number of equations
		Ref: http://en.wikipedia.org/wiki/Direct_linear_transformation
	*/
	CvMat *B = cvCreateMat(N, 6, CV_64FC1);
	for (int i = 0; i < N; i++)
	{
		B->data.db[i * 6 + 0] =  x[i * 2 + 1] * y[i * 3 + 0];
		B->data.db[i * 6 + 1] = -x[i * 2 + 0] * y[i * 3 + 0];
		B->data.db[i * 6 + 2] =  x[i * 2 + 1] * y[i * 3 + 1];
		B->data.db[i * 6 + 3] = -x[i * 2 + 0] * y[i * 3 + 1];
		B->data.db[i * 6 + 4] =  x[i * 2 + 1] * y[i * 3 + 2];
		B->data.db[i * 6 + 5] = -x[i * 2 + 0] * y[i * 3 + 2];
	}
	CvMat *X = cvCreateMat(6, 1, CV_64FC1);
	CvMat *Y = cvCreateMat(N, 1, CV_64FC1);
	cvSetZero(Y);
	cvSolve(B, Y, X, CV_SVD);
	a[0] = X->data.db[0];
	a[1] = X->data.db[2];
	a[2] = X->data.db[4];
	a[3] = X->data.db[1];
	a[4] = X->data.db[3];
	a[5] = X->data.db[5];

	cvReleaseMat(&Y);
	cvReleaseMat(&X);
	cvReleaseMat(&B);
}

/* Initial structure and motion */
int InitialStructAndMotion(	CvCapture* video,
							int* frame1,
							int* frame2,
							CvMat* projMatr1,
							CvMat* projMatr2,
							CvMat* points1,
							CvMat* points2,
							CvMat* status1,
							CvMat* status2,
							CvMat* objPoints4D)
{
	/* Initial struction and motion */
	/* Use the first key frame as a reference frame and fix its camera matrix to be [I 0] */
	/* Select the initial two frames to maximize the product of the number of matches and the
	image-based distance*/
	double prod = DBL_MIN;
	int i, j, curr;

	/* the total number of object points desired */
	int count;
	count = objPoints4D->cols;

	/* the two frames must be at least minStep away, have at least minMatch matches */
	int minMatch, minStep, frameCnt;

	minStep = 5;
	minMatch = count / 2;
	frameCnt = (int) cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_COUNT);

	/* book keeping variables */
	CvMat *initPoints1, *initPoints2, *initStatus1, *initStatus2;
	
	initPoints1 = cvCloneMat(points1);
	initPoints2 = cvCloneMat(points2);
	initStatus1 = cvCloneMat(status1);
	initStatus2 = cvCloneMat(status2);

	int initMatches;

	/* find out the maximizer */
	for (i = 0; i < frameCnt; i++)
	{
		cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, i);
		IplImage* image = cvCloneImage(cvQueryFrame(video));
		CreateFeaturePoints(image, points1, status1);
		
		for (j = i+minStep; j < frameCnt; j++)
		{
			int matches = FindCorrForGivenPoints(video, i, j, points1, status1,
				points2, status2, 1, 3.);
			if (matches < minMatch)	continue;
			double dist = PollefeysImageBasedDist(points1, status1, points2, status2);
			if (matches * dist > prod)
			{
				prod = matches * dist;
				*frame1 = i;
				*frame2 = j;
				initMatches = matches;
				cvCopy(points1, initPoints1);
				cvCopy(points2, initPoints2);
				cvCopy(status1, initStatus1);
				cvCopy(status2, initStatus2);
			}
		}

		cvReleaseImage(&image);
	}

	/* compact the points */
	CvMat *initPoints1_, *initPoints2_;
	
	initPoints1_ = cvCreateMat(3, initMatches, CV_64FC1);
	initPoints2_ = cvCreateMat(3, initMatches, CV_64FC1);

	double *ip1, *ip2, *ip1_, *ip2_;
	
	ip1 = initPoints1->data.db;
	ip2 = initPoints2->data.db;
	ip1_ = initPoints1_->data.db;
	ip2_ = initPoints2_->data.db;

	uchar *is1, *is2;
	
	is1 = initStatus1->data.ptr;
	is2 = initStatus2->data.ptr;

	curr = 0;
	for (i = 0; i < count; i++)
	{
		if (is1[i] && is2[i])
		{
			ip1_[curr + 0*initMatches] = ip1[0*count + i];
			ip1_[curr + 1*initMatches] = ip1[1*count + i];
			ip1_[curr + 2*initMatches] = 1.;
			
			ip2_[curr + 0*initMatches] = ip2[0*count + i];
			ip2_[curr + 1*initMatches] = ip2[1*count + i];
			ip2_[curr + 2*initMatches] = 1.;

			curr++;
		}
	}

	/* call the Hartley 2 view reconstruction procedure */

	cvSetZero(objPoints4D);
	int success = HartleyProjectReconstrFrom2View(initPoints1_, initPoints2_, projMatr1, projMatr2, objPoints4D);

	/* fill in the return value */
	if (success)
	{
		cvSetZero(points1);
		cvSetZero(points2);
		cvSetZero(status1);
		cvSetZero(status2);
		
		double *p1, *p2;
		
		p1 = points1->data.db;
		p2 = points2->data.db;
		
		curr = 0;
		for (i = 0; i < count; i++)
		{
			if (is1[i] && is2[i])
			{
				p1[0*count + curr] = ip1[0*count + i];
				p1[1*count + curr] = ip1[1*count + i];
				
				p2[0*count + curr] = ip2[0*count + i];
				p2[1*count + curr] = ip2[1*count + i];

				curr++;
			}
		}

		memset(status1->data.ptr, 1, sizeof(uchar)*curr);
		memset(status2->data.ptr, 1, sizeof(uchar)*curr);
	}

	/* free allocated memory */
	cvReleaseMat(&initPoints1);
	cvReleaseMat(&initPoints2);
	cvReleaseMat(&initStatus1);
	cvReleaseMat(&initStatus2);
	cvReleaseMat(&initPoints1_);
	cvReleaseMat(&initPoints2_);

	return success;
}
/* Select a new key frame near the current frame */
int NextKeyFrame(	CvCapture* video,
					const int frame1, 
					const int step, 
					const int minFrameStep, 
					const CvMat* points1, 
					const CvMat* status1,
					CvMat* points2, 
					CvMat* status2)
{
	int i, frame2, currFrame, currNumVisPoints, numPoints, visPntCount1, frameCnt;

	visPntCount1 = cvCountNonZero(status1);
	numPoints = points1->cols;
	currFrame = frame1;
	frameCnt = (int) cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_COUNT);

	CvMat *currPoints1, *currStatus1, *currPoints2, *currStatus2;
	
	currPoints1 = cvCloneMat(points1);
	currStatus1 = cvCloneMat(status1);
	currPoints2 = points2;
	currStatus2 = status2;

	int w, h;
	w = (int) cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_WIDTH);
	h = (int) cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_HEIGHT);

	IplImage *grayImage1, *grayImage2, *pyrImage1, *pyrImage2;

	/* Create Ipl images */
    grayImage1 = cvCreateImage(cvSize(w,h),8,1);
    grayImage2 = cvCreateImage(cvSize(w,h),8,1);
    pyrImage1  = cvCreateImage(cvSize(w+3,h),8,1);
    pyrImage2  = cvCreateImage(cvSize(w+3,h),8,1);

	CvPoint2D32f *cornerPoints1, *cornerPoints2;
	char *status;
	float *errors;

	/* a dead loop */
	for(;;)
	{
		/* prepare for a new loop */
		currNumVisPoints = cvCountNonZero(currStatus1);
		/* grab current frame before doing the loop */
		if (currFrame == frame1)
		{
			cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, currFrame);
			cvCvtColor(cvQueryFrame(video),grayImage1,CV_BGR2GRAY);
		}
		cvSetCaptureProperty(video, CV_CAP_PROP_POS_FRAMES, currFrame + step);
		cvCvtColor(cvQueryFrame(video),grayImage2,CV_BGR2GRAY);

		/* allocate memory for LK optical flow */
		cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*currNumVisPoints);
		cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*currNumVisPoints);
		status = (char*)cvAlloc( sizeof(char)*currNumVisPoints);
		errors = (float*)cvAlloc( 2 * sizeof(float)*currNumVisPoints);

		for( i = 0; i < currNumVisPoints; i++ )
		{
			status[i] = 1;
		}

		/* Copy points good points from input data */
		uchar *stat1 = currStatus1->data.ptr;
		uchar *stat2 = currStatus2->data.ptr;
		int curr = 0;
		for( i = 0; i < numPoints; i++ )
		{
			if( stat1[i] )
			{
				cornerPoints1[curr].x = (float)cvmGet(currPoints1,0,i);
				cornerPoints1[curr].y = (float)cvmGet(currPoints1,1,i);
				curr++;
			}
		}

		/* Define number of levels of pyramid */
		if (currFrame == frame1)
			cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
									pyrImage1, pyrImage2,
									cornerPoints1, cornerPoints2,
									currNumVisPoints, cvSize(10,10), 3,
									status, errors, 
									cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
									0/*CV_LKFLOW_PYR_A_READY*/ );
		else
			cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
									pyrImage1, pyrImage2,
									cornerPoints1, cornerPoints2,
									currNumVisPoints, cvSize(10,10), 3,
									status, errors, 
									cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
									CV_LKFLOW_PYR_A_READY );

		memset(stat2,0,sizeof(uchar)*numPoints);

	    int currVis = 0;
		int totalCorns = 0;

		/* Copy new points and set status */
		/* stat1 may not be the same as stat2 */
		for( i = 0; i < numPoints; i++ )
		{
			if( stat1[i] )
			{
				if( status[currVis] && errors[currVis] < 1000 )
				{
					stat2[i] = 1;
					cvmSet(currPoints2,0,i,cornerPoints2[currVis].x);
					cvmSet(currPoints2,1,i,cornerPoints2[currVis].y);
					totalCorns++;
				}
				currVis++;
			}
		}

		/* free allocated memory */
		cvFree(&cornerPoints1);
		cvFree(&cornerPoints2);
		cvFree(&status);
		cvFree(&errors);

		/* try to break the loop */
		frame2 = currFrame + step;
		if (totalCorns < 0.5*visPntCount1 && abs(frame2-frame1) >= minFrameStep)
		{
			cvCopy(currPoints2, points2);
			cvCopy(currStatus2, status2);
			break;
		}
		if (frame2 == 0 || frame2 == frameCnt)
		{
			// reach the border of the video
			frame2 = -1;
			break;
		}

		/* prepare for the next iteration */
		currFrame += step;
		swap(grayImage1, grayImage2);
		swap(pyrImage1, pyrImage2);
		swap(currPoints1, currPoints2);
		swap(currStatus1, currStatus2);
	}

	/* free allocated memory */
	cvReleaseMat(&currPoints1);
	cvReleaseMat(&currStatus1);
	cvReleaseImage(&grayImage1);
	cvReleaseImage(&grayImage2);
	cvReleaseImage(&pyrImage1);
	cvReleaseImage(&pyrImage2);

	return frame2;
}
/* projective reconstruction from a video, this function puts things together 
keyframes is an integer array, containing the indices of key frames
projMatr is Nx12, N camera matrices
objPoints4D is 4xM, M object points */
int ProjectReconstrFromVideo(CvCapture* video, CvMat* keyframes, CvMat* projMatr, CvMat* objPoints4D, const int minFrameStep)
{
	int count, i, j, success;

	count = objPoints4D->cols;

	/* initial structure and motion */
	int *kf, kfNeeded, kfCount;

	kf = keyframes->data.i;
	kfNeeded = keyframes->cols;
	kfCount = 0;

	CvMat projMatr1, projMatr2, **points, **status;

	projMatr1 = cvMat(3, 4, CV_64FC1, projMatr->data.db);
	projMatr2 = cvMat(3, 4, CV_64FC1, projMatr->data.db + 12);

	/* allocate memory for points and status */
	points = (CvMat**)cvAlloc(kfNeeded*sizeof(CvMat*));
	status = (CvMat**)cvAlloc(kfNeeded*sizeof(CvMat*));
	for (i = 0; i < kfNeeded; i++)
	{
		points[i] = cvCreateMat(2, count, CV_64FC1);
		status[i] = cvCreateMat(1, count, CV_8U);
	}

	int objPntCount;
	double *obj_p;
	obj_p = objPoints4D->data.db;

	success = InitialStructAndMotion(video, &kf[0], &kf[1], &projMatr1, &projMatr2, points[0], points[1], status[0], status[1], objPoints4D);
	if (success)
	{
		kfNeeded -= 2;
		kfCount += 2;
		objPntCount = cvCountNonZero(status[0]);
	}
	else
		return 0;

	/* choose other key frames and add them to the keyframe set */
	int nextKF, currKF, currKFIdx, newFPntsCount;
	CvMat *newFPoints2D1, *newFPoints2D2, *newFPointsStatus, newProjMatr, *newObjPoints;

	newFPoints2D1 = cvCreateMat(2, count, CV_64FC1);
	newFPoints2D2 = cvCreateMat(2, count, CV_64FC1);
	newFPointsStatus = cvCreateMat(2, count, CV_8U);
	
	/* select from the 0th key frame to the beginning of the video */
	int step;

	for (step = -1; step < 2; step = -step)
	{
		currKF = kf[0];
		currKFIdx = 0;
		for (;;)
		{
			if (kfNeeded <= 0)	break;
			nextKF = NextKeyFrame(video, currKF, step, minFrameStep, points[currKFIdx], status[currKFIdx], points[kfCount], status[kfCount]);
			if (nextKF == -1)	break;
			if (nextKF == kf[1])
			{
				currKF = kf[1];
				currKFIdx = 1;
				continue;
			}
		
			newProjMatr = cvMat(3, 4, CV_64FC1, projMatr->data.db + kfCount*12);
			AddNewFrameToPrevious(video, nextKF, currKF, points[currKFIdx], status[currKFIdx], objPoints4D,
				points[kfCount], status[kfCount], newFPoints2D1, newFPoints2D2, newFPointsStatus, &newProjMatr, 1, 3.);

			/* triangulate new 3D points */
			/* compact new feature points */
			newFPntsCount = cvCountNonZero(newFPointsStatus);

			CvMat *projPoints1, *projPoints2;

			projPoints1 = cvCreateMat(2, newFPntsCount, CV_64FC1);
			projPoints2 = cvCreateMat(2, newFPntsCount, CV_64FC1);
		
			double *pp1, *pp2, *fp1, *fp2;

			pp1 = projPoints1->data.db;
			pp2 = projPoints2->data.db;
			fp1 = newFPoints2D1->data.db;
			fp2 = newFPoints2D2->data.db;

			int curr = 0;
			for (i = 0; i < count; i++)
			{
				if (newFPointsStatus->data.ptr[i])
				{
					pp1[0*newFPntsCount + curr] = fp1[0*count + i];
					pp1[1*newFPntsCount + curr] = fp1[1*count + i];
					pp2[0*newFPntsCount + curr] = fp2[0*count + i];
					pp2[1*newFPntsCount + curr] = fp2[1*count + i];
					curr++;
				}
			}

			/* call the triangulation function */
			projMatr1 = cvMat(3, 4, CV_64FC1, projMatr->data.db + currKFIdx*12);
			projMatr2 = cvMat(3, 4, CV_64FC2, projMatr->data.db + kfCount*12);
			newObjPoints = cvCreateMat(4, newFPntsCount, CV_64FC1);

			cvTriangulatePoints(&projMatr1, &projMatr2, projPoints1, projPoints2, newObjPoints);

			/* add triangulated points to object points set */
			for (i = 0; i < newFPntsCount; i++)
			{
				for (j = 0; j < 4; j++)
				{
					obj_p[j*count + objPntCount] = newObjPoints->data.db[j*newFPntsCount + i];
				}
				for (j = 0; j < 2; j++)
				{
					points[currKFIdx]->data.db[j*count + objPntCount] = pp1[j*newFPntsCount + i];
					points[kfCount]->data.db[j*count + objPntCount] = pp2[j*newFPntsCount + i];
				}
				status[currKFIdx]->data.db[objPntCount] = 1;
				status[kfCount]->data.db[objPntCount] = 1;
				objPntCount++;
			}

			/* prepare for the next loop */
			currKFIdx = kfCount;
			currKF = nextKF;
			kfCount++;

			/* free allocated memory */
			cvReleaseMat(&projPoints1);
			cvReleaseMat(&projPoints2);
			cvReleaseMat(&newObjPoints);
		}
	}

	/* Bundle Adjustment */
	/* defining parameters for the sba function */
	int n;			// the number of 3D points
	int ncon;		// the number of 3D points whose parameters should not be modified
	int m;			// the number of cameras
	int mcon;		// the number of cameras whose parameters should not be modified
	char *vmask;	// point visibility mask
	double *p;		// parameter vector
	int cnp;		// the number of parameters defining a single camera
	int pnp;		// the number of parameters defining a single 3D point
	double *x;		// measurement vector
	double *covx;	// covariance matrix of x
	int mnp;		// the number of parameters defining an image point
	int *adata;	// additional data
	int itmax;		// maximum number of LM iterations
	int verbose;	// verbosity level
	double opts[SBA_OPTSSZ];	// minimization options for LM
	double info[SBA_INFOSZ];	// information regarding the outcome of the minimization

	/* prepare data */
	int NZ;

	NZ = 0;
	n = objPntCount;
	ncon = 0;
	m = kfCount;
	mcon = 1;

	vmask = (char*) malloc(sizeof(char)*m*n);
	for (j = 0; j < m; j++)
	{
		for (i = 0; i < n; i++)
		{
			if (status[j]->data.ptr[i])
			{
				vmask[i*m + j] = 1;
				NZ++;
			}
			else
			{
				vmask[i*m + j] = 0;
			}
		}
	}

	cnp = 11;
	pnp = 3;
	
	p = (double*) malloc(sizeof(double) * (m*cnp + n*pnp));
	for (i = 0; i < m; i++)
	{
		for (j = 0; j < cnp; j++)
		{
			p[i*cnp + j] = projMatr->data.db[i*12 + j];
		}
	}
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < pnp; j++)
		{
			p[m*cnp + i*pnp + j] = objPoints4D->data.db[j*count + i];
		}
	}

	mnp = 2;

	x = (double*) malloc(sizeof(double)*NZ*mnp);
	int curr = 0;
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < m; j++)
		{
			if (vmask[i*m + j])
			{
				x[curr++] = points[j]->data.db[0*count + i];
				x[curr++] = points[j]->data.db[1*count + i];
			}
		}
	}

	covx = 0;

	adata = (int*) malloc(sizeof(int)*2);
	adata[0] = m;
	adata[1] = n;

	itmax = 150;
	verbose = 10;
	opts[0] = 1e-3;
	opts[1] = opts[2] = opts[3] = 1e-12;
	opts[4] = 0.0;
	
	sba_motstr_levmar_x(n, ncon, m, mcon, 
		vmask, p, cnp, pnp, x, covx, mnp, &proj_func, &proj_fjac, adata, itmax, verbose, opts, info);
	
	/* free allocated memory */
	for (i = 0; i < kfNeeded; i++)
	{
		cvReleaseMat(&points[i]);
		cvReleaseMat(&status[i]);
	}
	cvFree(points);
	cvFree(status);

	cvReleaseMat(&newFPoints2D1);
	cvReleaseMat(&newFPoints2D2);
	cvReleaseMat(&newFPointsStatus);
	
	return 1;
}
/* Linear self-calibration by Marc Pollefeys */
void PollefeysLinearSelfCalibration(const CvMat* projMatr, const int w, const int h, CvMat* dualAbsQuad)
{
	int count, i, j, k, converge;
	double *miu, *p[3], *a;

	count = projMatr->rows / 3;
	miu = (double*) malloc(sizeof(double) * count);

	/* solve the linear least squares Ax = b*/
	CvMat *A, *U, *S, *V, *U_, *S_, *V_, *P3T, *PN, KN;

	A = cvCreateMat(6*count, 10, CV_64FC1);
	U = cvCreateMat(6*count, 10, CV_64FC1);
	S = cvCreateMat(10, 1, CV_64FC1);
	V = cvCreateMat(10, 10, CV_64FC1);

	U_ = cvCreateMat(4, 4, CV_64FC1);
	S_ = cvCreateMat(4, 4, CV_64FC1);
	V_ = cvCreateMat(4, 4, CV_64FC1);
	
	P3T = cvCreateMat(4, 1, CV_64FC1);
	PN = cvCloneMat(projMatr);

	/* normalize the projection matrices */
	double KN_dat[9] = {1.*(w+h), 0., w*1./2.,
						0., 1.*(w+h), h*1./2.,
						0., 0., 1.};
	KN = cvMat(3, 3, CV_64F, KN_dat);
	
	cvInvert(&KN, &KN);

	for (i = 0; i < count; i++)
	{
		CvMat P = cvMat(3, 4, CV_64F, PN->data.db + i*3*4);
		cvMatMul(&KN, &P, &P);
	}

	/* fill matrix A */
	for (i = 0; i < count; i++)
	{
		/* 3 rows of the ith projection matrix */
		p[0] = PN->data.db + i*3*4;
		p[1] = p[0] + 4;
		p[2] = p[1] + 4;

		a = A->data.db + i*6*10;

		for (j = 0; j < 3; j++)
		{
			k = (j+1) % 3;
			a[j*10 + 0] = p[j][0]*p[j][0] - p[k][0]*p[k][0];
			a[j*10 + 1] = 2*(p[j][0]*p[j][1] - p[k][0]*p[k][1]);
			a[j*10 + 2] = 2*(p[j][0]*p[j][2] - p[k][0]*p[k][2]);
			a[j*10 + 3] = 2*(p[j][0]*p[j][3] - p[k][0]*p[k][3]);
			a[j*10 + 4] = p[j][1]*p[j][1] - p[k][1]*p[k][1];
			a[j*10 + 5] = 2*(p[j][1]*p[j][2] - p[k][1]*p[k][2]);
			a[j*10 + 6] = 2*(p[j][1]*p[j][3] - p[k][1]*p[k][3]);
			a[j*10 + 7] = p[j][2]*p[j][2] - p[k][2]*p[k][2];
			a[j*10 + 8] = 2*(p[j][2]*p[j][3] - p[k][2]*p[k][3]);
			a[j*10 + 9] = p[j][3]*p[j][3] - p[k][3]*p[k][3];
		}

		a += 3*10;

		for (j = 0; j < 3; j++)
		{
			k = (j+1) % 3;
			a[j*10 + 0] = p[j][0]*p[k][0];
			a[j*10 + 1] = p[j][1]*p[k][0] + p[j][0]*p[k][1];
			a[j*10 + 2] = p[j][2]*p[k][0] + p[j][0]*p[k][2];
			a[j*10 + 3] = p[j][3]*p[k][0] + p[j][0]*p[k][3];
			a[j*10 + 4] = p[j][1]*p[k][1];
			a[j*10 + 5] = p[j][1]*p[k][2] + p[j][2]*p[k][1];
			a[j*10 + 6] = p[j][1]*p[k][3] + p[j][3]*p[k][1];
			a[j*10 + 7] = p[j][2]*p[k][2];
			a[j*10 + 8] = p[j][2]*p[k][3] + p[j][3]*p[k][2];
			a[j*10 + 9] = p[j][3]*p[k][3];
		}
	}

	/* iteratively solve the linear least squares */
	memset(miu, 1, sizeof(miu));

	for (;;)
	{
		/* weight the matrix A */

		for (i = 0; i < count; i++)
		{
			double weight[6] = {0.2*miu[i], 9*miu[i], 9*miu[i], 0.1*miu[i], 0.01*miu[i], 0.1*miu[i]};
		
			a = A->data.db + i*6*10;
			for (j = 0; j < 6; j++)
			{
				for (k = 0; k < 10; k++)
				{
					a[j*10 + k] /= weight[j];
				}
			}
		}

		/* solve the linear least squares */
		cvSVD(A, S, U, V);
		k = 0;
		for (i = 0; i < 4; i++)
		{
			for (j = i; j < 4; j++)
			{
				dualAbsQuad->data.db[i*4 + j] = V->data.db[k*10 + 9];
				dualAbsQuad->data.db[j*4 + i] = V->data.db[k*10 + 9];
				k++;
			}
		}

		/* enforcing the rank 3 constraint */
		cvSVD(dualAbsQuad, S_, U_, V_, CV_SVD_MODIFY_A + CV_SVD_V_T);
		S_->data.db[15] = 0.;
		cvMatMul(S_, V_, dualAbsQuad);
		cvMatMul(U_, dualAbsQuad, dualAbsQuad);

		/* compute new miu factors */
		double tmp_miu;

		converge = 0;
		for (i = 0; i < count; i++)
		{
			CvMat P3 = cvMat(1, 4, CV_64F, projMatr->data.db + i*3*4 + 2*4);
			cvMatMul(&P3, dualAbsQuad, &P3);
			cvT(&P3, P3T);
			tmp_miu = cvDotProduct(&P3, P3T);
			if (fabs(tmp_miu - miu[i]) < FLT_EPSILON)
			{
				converge++;
			}
			else
			{
				miu[i] = tmp_miu;
			}
		}

		/* all miu factors converge */
		if (converge == count)
			break;
	}

	cvReleaseMat(&A);
	cvReleaseMat(&U);
	cvReleaseMat(&S);
	cvReleaseMat(&V);
	cvReleaseMat(&U_);
	cvReleaseMat(&S_);
	cvReleaseMat(&V_);
	cvReleaseMat(&P3T);

	return;
}
/* non-linear self-calibration */

/* global variable for LM */
CvMat *P_MAT;

void fcn_pnlsc(int *m, int *n, double *x, double *fvec, double *fjac, int *ldfjac, int *iflag)
{
	int i, j, l;

	double k[9] = {
		x[0], x[1], x[3],
		0., x[2]*x[0], x[4],
		0., 0., 1.
	};
	double a_dat[3] = {-x[5], -x[6], -x[7]};
	double scale;

	CvMat K, *P, *Q, a, aT, *KKT, *PQPT, *R, *tmp;

	K = cvMat(3, 3, CV_64F, k);
	a = cvMat(3, 1, CV_64F, a_dat);
	aT = cvMat(1, 3, CV_64F, a_dat);
	P = cvCreateMat(3, 4, CV_64FC1);
	Q = cvCreateMat(4, 4, CV_64FC1);
	KKT = cvCreateMat(3, 3, CV_64FC1);
	PQPT = cvCreateMat(3, 3, CV_64FC1);
	R = cvCreateMat(3, 3, CV_64FC1);

	/* compute Q from K and a */
	cvTranspose(&K, KKT);
	cvMatMul(&K, KKT, KKT);

	cvGetSubRect(Q, tmp, cvRect(0, 0, 3, 3));
	cvCopy(KKT, tmp);

	cvGetSubRect(Q, tmp, cvRect(3, 0, 1, 3));
	cvMatMul(KKT, &a, tmp);

	cvGetSubRect(Q, tmp, cvRect(0, 3, 3, 1));
	cvMatMul(&aT, KKT, tmp);

	Q->data.db[15] = cvDotProduct(tmp, &a);

	if (*iflag == 1)
	{
		/* scale K*Kt */
		scale = 1. / cvNorm(KKT, 0, CV_L2, 0);
		tmp = cvCreateMat(3, 3, CV_64FC1);
		cvScale(KKT, tmp, scale);
		cvCopy(tmp, KKT);
		cvReleaseMat(&tmp);

		for (i = 0; i < *m; i++)
		{
			/* compute PQPT */
			cvGetSubRect(P_MAT, tmp, cvRect(0, i*3, 4, 3));
			cvCopy(tmp, P);
			
			cvMatMul(P, Q, PQPT);
			
			cvTranspose(tmp, P);
			cvMatMul(PQPT, P, PQPT);

			scale = 1. / cvNorm(PQPT, 0, CV_L2, 0);
			tmp = cvCreateMat(3, 3, CV_64FC1);
			cvScale(PQPT, tmp, scale);
			cvCopy(tmp, PQPT);
			cvReleaseMat(&tmp);

			cvSub(KKT, PQPT, R);
			
			scale = cvNorm(R, 0, CV_L2, 0);
			fvec[i] = (scale*scale) / 2.;
		}
	}
	else if (*iflag == 2)
	{
		/* compute the Jacobian matrix dQ/dx */
		double f, s, r, u, v, a1, a2, a3;
		
		f = x[0];
		s = x[1];
		r = x[2];
		u = x[3];
		v = x[4];
		a1 = x[5];
		a2 = x[6];
		a3 = x[7];

		double dqdx[8*4*4] = {
			/* dq/df */
			2*f, s*r, 0., 0.,//-(2*a1*f + s*r*a2),
			0., 2*r*r*f, 0., 0.,//-(s*r*a1 + 2*r*r*f*a2),
			0., 0., 0., 0.,
			0., 0., 0., 0.,
			/* dq/ds */
			2*s, r*f, 0., 0.,
			0., 0., 0., 0.,
			0., 0., 0., 0.,
			0., 0., 0., 0.,
			/* dq/dr */
			0., s*f, 0., 0.,
			0., 2*f*f*r, 0., 0.,
			0., 0., 0., 0.,
			0., 0., 0., 0.,
			/* dq/du */
			2*u, v, 1., 0.,
			0., 0., 0., 0.,
			0., 0., 0., 0.,
			0., 0., 0., 0.,
			/* dq/dv */
			0., u, 0., 0.,
			0., 2*v, 1., 0.,
			0., 0., 0., 0.,
			0., 0., 0., 0.
			/* dq/da1 */
			/* dq/da2 */
			/* dq/da3 */
		};

		CvMat *DQDX, *tmp2;

		DQDX = &cvMat(8*4, 4, CV_64F, dqdx);

		for (i = 0; i < 5; i++)
		{
			/* vector a as constant */
			cvGetSubRect(DQDX, tmp, cvRect(0, i*4, 3, 3));
			cvGetSubRect(DQDX, tmp2, cvRect(3, i*4, 1, 3));
			cvMatMul(tmp, &a, tmp2);

			dqdx[i*4*4 + 15] = cvDotProduct(&aT, tmp2);
		}
		for (i = 5, j = 0; i < 8; i++, j++)
		{
			cvGetCol(KKT, tmp, j);
			cvGetSubRect(DQDX, tmp2, cvRect(3, i*4, 1, 3));
			cvScale(tmp, tmp2, -1.);

			dqdx[i*4*4 + 15] = -Q->data.db[j*4 + 3] - cvDotProduct(&aT, tmp2);
		}
		
		/* complete the symmetry matrix */
		for (i = 0; i < 8; i++)
		{
			for (j = 0; j < 4; j++)
			{
				for (l = 0; l < j; l++)
				{
					dqdx[i*4*4 + j*4 + l] = dqdx[i*4*4 + l*4 + j];
				}
			}
		}

		/* compute the Jacobian */

	}

	cvReleaseMat(&P);
	cvReleaseMat(&Q);
	cvReleaseMat(&KKT);
	cvReleaseMat(&PQPT);
	cvReleaseMat(&R);
}
void PollefeysNonLinearSelfCalibration(const CvMat* projMatr, const CvMat* dualAbsQuad, CvMat* K)
{
	int i, j;
	double x[8], *q, scale;

	CvMat *Q;

	Q = cvCreateMat(4, 4, CV_64FC1);

	/* force Q_3_3 to 1 */
	scale = 1. / dualAbsQuad->data.db[10];
	cvScale(dualAbsQuad, Q, scale);

	/* compute initial params from dualAbsQuad */
	/* 0-----f 
	   1-----s
	   2-----r
	   3-----u
	   4-----v
	   5-----a1
	   6-----a2
	   7-----a3*/
	q = Q->data.db;

	x[3] = q[2] / q[10];
	x[4] = q[6] / q[10];
	x[1] = (q[1] - x[3]*x[4]) / (sqrt(q[5] - x[4]*x[4]));
	x[0] = sqrt(q[0] - x[3]*x[3] - x[1]*x[1]);
	x[2] = (q[1] - x[3]*x[4]) / (x[0]*x[1]);
	
	/* solve an over determined linear system for vector a */
	CvMat a, *A, *b, tmp;

	a = cvMat(3, 1, CV_64F, &x[5]);
	A = cvCreateMat(4, 3, CV_64FC1);
	b = cvCreateMat(4, 1, CV_64FC1);

	cvGetSubRect(Q, &tmp, cvRect(0, 0, 4, 3));
	cvCopy(&tmp, A);
	cvGetSubRect(Q, &tmp, cvRect(3, 0, 1, 3));
	cvCopy(&tmp, b);

	cvSolve(A, b, &a, CV_SVD);

	x[5] = -x[5];
	x[6] = -x[6];
	x[7] = -x[7];

	/* end computing initial params */


	cvReleaseMat(&Q);
	cvReleaseMat(&A);
	cvReleaseMat(&b);
}